#include "abs_encoder.h"
#include "elmo.h"
#include "joint.h"
static int servo_switch = 0;
struct joint_t
{
    enum  joint_id id;
    float rad2line;
    float degree2line;
    float motor_dir;
    float abs_dir;
    int   rotation_ratio;
    float normal_torque;
    float upper_limit;
    float lower_limit;
    //abs_encoder's value when virtual axis is 0 
    float abs_encoder_offset;
    //abs_encoder's value when motor is 0
    float motor_offset;  
    float current_position ;
    float current_velocity ;
    float current_torque ;
    float aim_position ;
    float aim_velocity ;
    float P ;
};

static int this_id;
static struct joint_t joints[JOINT_NUMBER]=
{
    {IDEL_ID},
    
    {LEFT_HIP, 
     HIP_RAD_RATIO, HIP_DEGREE_RATIO, 
     NAGTIVE,  NAGTIVE , 
     HIP_RATIO,NORMAL_TORQUE, 
     HIP_UPPER_LIMIT, HIP_LOWER_LIMIT,        
     LEFT_HIP_OFFSET,0.0, 
     0.0, 0.0, 0.0,
     0.0, 0.0, 10000.0},
    
    {LEFT_KNEE, 
     KNEE_RAD_RATIO, KNEE_DEGREE_RATIO, 
     NAGTIVE,  NAGTIVE , 
     KNEE_RATIO, NORMAL_TORQUE,
     KNEE_UPPER_LIMIT,KNEE_LOWER_LIMIT,
     LEFT_KNEE_OFFSET,0.0, 
     0.0, 0.0, 0.0, 
     0.0, 0.0, 10000.0},
    
     {RIGHT_HIP, 
     HIP_RAD_RATIO, HIP_DEGREE_RATIO, 
     POSITIVE,  POSITIVE , 
     HIP_RATIO, NORMAL_TORQUE, 
     HIP_UPPER_LIMIT,HIP_LOWER_LIMIT,        
     RIGHT_HIP_OFFSET,0.0, 
     0.0, 0.0, 0.0,
     0.0, 0.0, 10000.0},
    
    {RIGHT_KNEE , 
     KNEE_RAD_RATIO, KNEE_DEGREE_RATIO, 
     POSITIVE,  POSITIVE , 
     KNEE_RATIO, NORMAL_TORQUE,
     KNEE_UPPER_LIMIT,KNEE_LOWER_LIMIT,
     RIGHT_KNEE_OFFSET,0.0, 
     0.0, 0.0, 0.0, 
     0.0, 0.0, 10000.0},
};

//range of angle (-180,180)
float calcu_virtual_axis_angle(float abs, float offset)
{
    float tmp = 0.0 ;
    float angle = 0.0;
    if(offset > 180.0)
    {
        //increase 360->0->5
        tmp = offset + 180.0 - 2*180.0;
        if(abs>0&&abs<tmp)
        {
            angle =  2*180.0 - offset + abs;
        }
        else
        {
            angle = abs - offset;
        }
    }
    else
    {
        tmp = offset - 180.0;
        // decrease to 0 -> 360->355->350
       if( (abs> (2*180.0 + tmp)) && abs < 2*180.0 )
       {
            angle = -offset - (180.0 - abs);
       }
       else
       {
            angle = abs - offset;
       }
    }
    return angle*joints[this_id].abs_dir;
}

void joint_init(enum joint_id id)
{   
    float abs = 0.0 ; 
    float elmo_angle = 0.0 ;
    float current_angle = 0.0 ;
    this_id = id ; 
    abs = get_abs_encoder() ;
    current_angle = calcu_virtual_axis_angle(abs, joints[this_id].abs_encoder_offset);
    elmo_angle = (float)elmo_get_position() ;
    joints[this_id].motor_offset = elmo_angle/(float)joints[this_id].degree2line - current_angle;
}

float joint_get_angle(void)
{
    return joints[this_id].current_position;
}

float joint_get_velocity(void)
{
    return joints[this_id].current_velocity;
}

float joint_get_torque(void)
{
    return joints[this_id].current_torque;
}

void joint_data_update(void)
{
    float elmo_angle = 0.0 ;
    float abs = get_abs_encoder() ;
    elmo_angle = (float)elmo_get_position() ;
    joints[this_id].current_position =  joints[this_id].motor_dir*elmo_angle/(float)joints[this_id].degree2line - joints[this_id].motor_offset;
    joints[this_id].current_velocity = (float)elmo_get_velocity()*joints[this_id].motor_dir/(float)joints[this_id].degree2line;
    joints[this_id].current_torque = (float)elmo_get_current()*joints[this_id].motor_dir/1000.0*joints[this_id].normal_torque*joints[this_id].rotation_ratio;  
}

void joint_servo_update(void)
{
    if(servo_switch==1)
    {
        float error_position = joints[this_id].aim_position - joints[this_id].current_position;
        float aim_velocity = joints[this_id].P*error_position*joints[this_id].motor_dir;
        int aim_vel = (int)aim_velocity;
       // elmo_run_velocity(aim_vel, 1500000, 1500000, 500000);
    }
   
}
void set_joint_servo_on()
{
    servo_switch = 1;
}
void set_joint_servo_off()
{
    servo_switch = 0;
}
void set_joint_aim_angle(float degree)
{
    joints[this_id].aim_position = degree;
}
